/**
 * @ Author: luoqi
 * @ Create Time: 2024-04-01 22:17
 * @ Modified by: luoqi
 * @ Modified time: 2024-04-02 23:19
 * @ Description:
 */

#define DBG_TAG "IMU"
#define DBG_LVL DBG_INFO
#include <rtdbg.h>
#include <rtthread.h>
#include <rtdevice.h>
#include <stdbool.h>
#include "imu.h"
#include "../common.h"
#include "../devices/blc.h"
#include "../modules/jy901.h"

static rt_device_t uart6;
static struct serial_configure config = RT_SERIAL_CONFIG_DEFAULT;
static rt_mq_t uart6_rx_mq;

static BlcObj *_blc = NULL;

typedef struct rx_msg {
    rt_device_t dev;
    rt_size_t size;
} RxdMsg;

static rt_err_t _uart6_recv_cb(rt_device_t dev, rt_size_t size)
{
    RxdMsg msg;
    msg.dev = dev;
    msg.size = size;

    rt_err_t err = rt_mq_send(uart6_rx_mq, &msg, sizeof(msg));
    if(err == -RT_EFULL) {
        LOG_W("uart6 rx mq full");
    }

    return err;
}

static bool is_show_raw = false;

static void _task_imu_hdl(void *args)
{
    RxdMsg msg;
    uint8_t buf[64];
    while(1) {
        rt_err_t mqsz = rt_mq_recv(uart6_rx_mq, &msg, sizeof(msg), RT_WAITING_FOREVER);
        if(mqsz > 0) {
            int sz = rt_device_read(uart6, 0, buf, msg.size);
            if(is_show_raw) {
                INFO(" mqsz: %d, sz: %d - ", mqsz, sz);
                SHOW_DATA(buf, sz);
            }
            if(sz > 0) {
                jy901_decode(&_blc->imu, buf, sz);
            }
        }
    }
}

int imu_init(void)
{
    uart6 = rt_device_find("uart6");
    rt_device_control(uart6, RT_DEVICE_CTRL_CONFIG, &config);
    rt_device_open(uart6, RT_DEVICE_FLAG_TX_NON_BLOCKING | RT_DEVICE_FLAG_RX_NON_BLOCKING);
    rt_device_set_rx_indicate(uart6, _uart6_recv_cb);

    uart6_rx_mq = rt_mq_create("uart6_rx", 16, 16, RT_IPC_FLAG_FIFO);

    rt_thread_t task = rt_thread_create("imu", _task_imu_hdl, RT_NULL, 1024, 5, 2);
    if(task != RT_NULL) {
        rt_thread_startup(task);
        LOG_I("imu task startup ...");
    } else {
        LOG_E("imu task create error ...");
        return -1;
    }

    _blc = blc_get();

    return 0;
}

static void _cmd_imu_help(void)
{
    MSG(" -raw [on/off]");
}

static int _cmd_imu(int argc, char **argv)
{
    if(argc == 1) {
        MSG(" accx: %.3f, accy: %.3f, accz: %.3f", _blc->imu.acc_x, _blc->imu.acc_y, _blc->imu.acc_z);
        MSG(" wx:   %.3f, wy:   %.3f, wz:   %.3f", _blc->imu.w_x, _blc->imu.w_y, _blc->imu.w_z);
        MSG(" aglx: %.3f, agly: %.3f, aglz: %.3f", _blc->imu.agl_x, _blc->imu.agl_y, _blc->imu.agl_z);
    } else if(ISARG(argv[1], "?") && argc == 2) {
        _cmd_imu_help();
    } else if(ISARG(argv[1], "raw") && argc == 3) {
        if(ISARG(argv[2], "on")) {
            is_show_raw = true;
        } else if(ISARG(argv[2], "off")) {
            is_show_raw = false;
        } else {
            ARG_ERR("raw");
        }
    } else {
        ARG_ERR("imu");
    }
    return 0;
}
MSH_CMD_EXPORT_ALIAS(_cmd_imu, imu, imu command);
